#include "CustomDS.h"

CustomDS::CustomDS(const Drive * driving) : 
	MovingArmSideLimitSwitch(14),
	//Arm1RetractedReedSwitch(6),  analog
	Arm1ExtendedReedSwitch(10),
	//Arm2RetractedReedSwitch(8),  analog
	Arm2ExtendedReedSwitch(11),
	StationaryArm1RetractedSwitch(6),
	StationaryArm2RetractedSwitch(7),
	StationaryArm1ExtendedSwitch(8),
	StationaryArm2ExtendedSwitch(9),
	LateralExtendedSwitch(13),
	LateralRetractedSwitch(12),
	lateralSolenoid1(2, 4, 5),
	lateralSolenoid2(2, 6, 7),
	stationaryClawSolenoid1(6, 7),
	stationaryClawSolenoid2(2, 2, 3),
	armSolenoid1(2, 3),
	armSolenoid2(4, 5),
	DS(DriverStation::GetInstance()),
	DSIO(DS->GetEnhancedIO())
{
	BeamBreak1 = new AnalogChannel(1);
	BeamBreak2 = new AnalogChannel(2);
	Arm1RetractedReedSwitch = new AnalogChannel(3);
	Arm2RetractedReedSwitch = new AnalogChannel(4);
	dumper = new Victor(3);
	drive = driving;
	beamBreakVoltageBand = 4.0;
	reedSensorVoltageBand = 0.25;
	prevones = 0;
	prevzeros = 0;
	nextones = 0;
	nextzeros = 0;
}

void CustomDS::ResetLights()
{
	DSIO.SetDigitalOutput(10, CustomDS::Off);
	DSIO.SetDigitalOutput(11, CustomDS::Off);
	DSIO.SetDigitalOutput(12, CustomDS::Off);
	DSIO.SetDigitalOutput(13, CustomDS::Off);
	DSIO.SetDigitalOutput(14, CustomDS::Off);
	DSIO.SetDigitalOutput(16, CustomDS::Off);
}
void CustomDS::SetConfig()
{
	DSIO.SetDigitalConfig(1, DriverStationEnhancedIO::kInputPullUp);
	DSIO.SetDigitalConfig(2, DriverStationEnhancedIO::kInputPullUp);
	DSIO.SetDigitalConfig(3, DriverStationEnhancedIO::kInputPullUp);
	DSIO.SetDigitalConfig(4, DriverStationEnhancedIO::kInputPullUp);
	DSIO.SetDigitalConfig(5, DriverStationEnhancedIO::kInputPullUp);
	DSIO.SetDigitalConfig(6, DriverStationEnhancedIO::kInputPullUp);
	DSIO.SetDigitalConfig(7, DriverStationEnhancedIO::kInputPullUp);
	DSIO.SetDigitalConfig(8, DriverStationEnhancedIO::kInputPullUp);
	DSIO.SetDigitalConfig(9, DriverStationEnhancedIO::kOutput);
	DSIO.SetDigitalConfig(10, DriverStationEnhancedIO::kOutput);
	DSIO.SetDigitalConfig(11, DriverStationEnhancedIO::kOutput);
	DSIO.SetDigitalConfig(12, DriverStationEnhancedIO::kOutput);
	DSIO.SetDigitalConfig(13, DriverStationEnhancedIO::kOutput);
	DSIO.SetDigitalConfig(14, DriverStationEnhancedIO::kOutput);
	DSIO.SetDigitalConfig(15, DriverStationEnhancedIO::kInputPullUp);
	DSIO.SetDigitalConfig(16, DriverStationEnhancedIO::kOutput);
}

void CustomDS::InitClimb()
{
	ClimbingStateMachine = Start;
	previous.Data.Digs = 0;
	if(digitals.Data.bits.prevState == 1)
	{
		prevlastState = true;
	}
	else
	{
		prevlastState = false;
	}
	if(digitals.Data.bits.nextState == 1)
	{
		nextlastState = true;
	}
	else
	{
		nextlastState = false;
	}
}

void CustomDS::HandleComponents()
{
	digitals.Data.Digs = DSIO.GetDigitals();
	printf("Digitals %x\n", digitals.Data.Digs );
	this->HandleState();
	this->HandleButtonBox();
	this->UpdateDS();
}
void CustomDS::HandleState()
{

	switch(ClimbingStateMachine)
	{
	case Start:
		//printf("Start State");
		lateralSolenoid1.Set(DoubleSolenoid::kReverse);
		lateralSolenoid2.Set(DoubleSolenoid::kReverse);
		stationaryClawSolenoid1.Set(DoubleSolenoid::kReverse);
		stationaryClawSolenoid2.Set(DoubleSolenoid::kReverse);
		armSolenoid1.Set(DoubleSolenoid::kReverse);
		armSolenoid2.Set(DoubleSolenoid::kReverse);
		if((Arm1RetractedReedSwitch->GetVoltage() < reedSensorVoltageBand) && (Arm2RetractedReedSwitch->GetVoltage() < reedSensorVoltageBand) && (StationaryArm1RetractedSwitch.Get() == 0) && (StationaryArm2RetractedSwitch.Get() == 0) && (LateralRetractedSwitch.Get() == 0))
		{
			DSIO.SetDigitalOutput(13, CustomDS::On);	
		}
		break;
	case Floor:
		//printf("Floor State");
		lateralSolenoid1.Set(DoubleSolenoid::kOff);
		lateralSolenoid2.Set(DoubleSolenoid::kOff);
		stationaryClawSolenoid1.Set(DoubleSolenoid::kOff);
		stationaryClawSolenoid2.Set(DoubleSolenoid::kOff);
		armSolenoid1.Set(DoubleSolenoid::kForward);
		armSolenoid2.Set(DoubleSolenoid::kOff);
		if(Arm1ExtendedReedSwitch.Get() == 0)
		{
			DSIO.SetDigitalOutput(13, CustomDS::On);
		}
		break;
	case LiftToBar1:
		//printf("Bar 1 lift State");
		lateralSolenoid1.Set(DoubleSolenoid::kOff);
		lateralSolenoid2.Set(DoubleSolenoid::kOff);
		stationaryClawSolenoid1.Set(DoubleSolenoid::kOff);
		stationaryClawSolenoid2.Set(DoubleSolenoid::kOff);
		armSolenoid1.Set(DoubleSolenoid::kReverse);
		armSolenoid2.Set(DoubleSolenoid::kOff);
		if(Arm1RetractedReedSwitch->GetVoltage() < reedSensorVoltageBand)
		{
			DSIO.SetDigitalOutput(13, CustomDS::On);
		}
		break;
	case HandoffBar1:
		//printf("Bar 1 Handoff State");
		lateralSolenoid1.Set(DoubleSolenoid::kForward);
		lateralSolenoid2.Set(DoubleSolenoid::kForward);
		stationaryClawSolenoid1.Set(DoubleSolenoid::kOff);
		stationaryClawSolenoid2.Set(DoubleSolenoid::kOff);
		armSolenoid1.Set(DoubleSolenoid::kOff);
		armSolenoid2.Set(DoubleSolenoid::kOff);
		if((BeamBreak1->GetVoltage() > beamBreakVoltageBand) )//&& (BeamBreak2->GetVoltage() > beamBreakVoltageBand))
		{
			stationaryClawSolenoid1.Set(DoubleSolenoid::kForward);
			stationaryClawSolenoid2.Set(DoubleSolenoid::kForward);
			armSolenoid1.Set(DoubleSolenoid::kForward);
		}
		if((Arm1ExtendedReedSwitch.Get() == 0) && (Arm2ExtendedReedSwitch.Get() == 0) && (StationaryArm1ExtendedSwitch.Get() == 0) && (StationaryArm2ExtendedSwitch.Get() == 0) && (LateralExtendedSwitch.Get() == 0))
		{
			DSIO.SetDigitalOutput(13, CustomDS::On);
		}
		break;
	case ExtendToBar2:
		//printf("Bar 2 Extend State");
		lateralSolenoid1.Set(DoubleSolenoid::kForward);
		lateralSolenoid2.Set(DoubleSolenoid::kForward);
		stationaryClawSolenoid1.Set(DoubleSolenoid::kForward);
		stationaryClawSolenoid2.Set(DoubleSolenoid::kForward);
		armSolenoid1.Set(DoubleSolenoid::kForward);
		armSolenoid2.Set(DoubleSolenoid::kForward);
		if((Arm1ExtendedReedSwitch.Get() == 0) && (Arm2ExtendedReedSwitch.Get() == 0) && (StationaryArm1ExtendedSwitch.Get() == 0) && (StationaryArm2ExtendedSwitch.Get() == 0) && (LateralExtendedSwitch.Get() == 0))
		{
			DSIO.SetDigitalOutput(13, CustomDS::On);
		}
		break;
	case LiftAndReleaseOffBar1:
		//printf("Bar 1 Release State");
		lateralSolenoid1.Set(DoubleSolenoid::kReverse);
		lateralSolenoid2.Set(DoubleSolenoid::kReverse);
		stationaryClawSolenoid1.Set(DoubleSolenoid::kOff);
		stationaryClawSolenoid2.Set(DoubleSolenoid::kOff);
		armSolenoid1.Set(DoubleSolenoid::kOff);
		armSolenoid2.Set(DoubleSolenoid::kOff);
		if(MovingArmSideLimitSwitch.Get() == 0)
		{
			armSolenoid1.Set(DoubleSolenoid::kReverse);
			armSolenoid2.Set(DoubleSolenoid::kReverse);
			stationaryClawSolenoid1.Set(DoubleSolenoid::kReverse);
			stationaryClawSolenoid2.Set(DoubleSolenoid::kReverse);
		}
		if((Arm1RetractedReedSwitch->GetVoltage() < reedSensorVoltageBand) && (Arm2RetractedReedSwitch->GetVoltage() < reedSensorVoltageBand) && (LateralRetractedSwitch.Get() == 0) && (StationaryArm1RetractedSwitch.Get() == 0) && (StationaryArm2RetractedSwitch.Get() == 0))
		{
			DSIO.SetDigitalOutput(13, CustomDS::On);
		}
		break;
	case HandoffBar2:
		//printf("Bar 2 Handoff State");
		lateralSolenoid1.Set(DoubleSolenoid::kForward);
		lateralSolenoid2.Set(DoubleSolenoid::kForward);
		stationaryClawSolenoid1.Set(DoubleSolenoid::kOff);
		stationaryClawSolenoid2.Set(DoubleSolenoid::kOff);
		armSolenoid1.Set(DoubleSolenoid::kOff);
		armSolenoid2.Set(DoubleSolenoid::kOff);
		if((BeamBreak1->GetVoltage() > beamBreakVoltageBand) )//&& (BeamBreak2->GetVoltage() > beamBreakVoltageBand))
		{
			stationaryClawSolenoid1.Set(DoubleSolenoid::kForward);
			stationaryClawSolenoid2.Set(DoubleSolenoid::kForward);
			armSolenoid1.Set(DoubleSolenoid::kForward);
		}
		if((Arm1ExtendedReedSwitch.Get() == 0) && (Arm2ExtendedReedSwitch.Get() == 0) && (StationaryArm1ExtendedSwitch.Get() == 0) && (StationaryArm2ExtendedSwitch.Get() == 0) && (LateralExtendedSwitch.Get() == 0))
		{
			DSIO.SetDigitalOutput(13, CustomDS::On);
		}
		break;
	case ExtendToBar3:
		//printf("Bar 3 Extend State");
		lateralSolenoid1.Set(DoubleSolenoid::kForward);
		lateralSolenoid2.Set(DoubleSolenoid::kForward);
		stationaryClawSolenoid1.Set(DoubleSolenoid::kForward);
		stationaryClawSolenoid2.Set(DoubleSolenoid::kForward);
		armSolenoid1.Set(DoubleSolenoid::kForward);
		armSolenoid2.Set(DoubleSolenoid::kForward);
		if((Arm1ExtendedReedSwitch.Get() == 0) && (Arm2ExtendedReedSwitch.Get() == 0) && (StationaryArm1ExtendedSwitch.Get() == 0) && (StationaryArm2ExtendedSwitch.Get() == 0) && (LateralExtendedSwitch.Get() == 0))
		{
			DSIO.SetDigitalOutput(13, CustomDS::On);
		}
		break;
	case LiftAndReleaseOffBar2:
		//printf("Bar 2 Release State");
		lateralSolenoid1.Set(DoubleSolenoid::kReverse);
		lateralSolenoid2.Set(DoubleSolenoid::kReverse);
		stationaryClawSolenoid1.Set(DoubleSolenoid::kOff);
		stationaryClawSolenoid2.Set(DoubleSolenoid::kOff);
		armSolenoid1.Set(DoubleSolenoid::kOff);
		armSolenoid2.Set(DoubleSolenoid::kOff);
		if(MovingArmSideLimitSwitch.Get() == 0)
		{
			armSolenoid1.Set(DoubleSolenoid::kReverse);
			armSolenoid2.Set(DoubleSolenoid::kReverse);
			stationaryClawSolenoid1.Set(DoubleSolenoid::kReverse);
			stationaryClawSolenoid2.Set(DoubleSolenoid::kReverse);
		}
		if((Arm1RetractedReedSwitch->GetVoltage() < reedSensorVoltageBand) && (Arm2RetractedReedSwitch->GetVoltage() < reedSensorVoltageBand) && (LateralRetractedSwitch.Get() == 0) && (StationaryArm1RetractedSwitch.Get() == 0) && (StationaryArm2RetractedSwitch.Get() == 0))
		{
			DSIO.SetDigitalOutput(13, CustomDS::On);
		}
		break;
	case HandoffBar3:
		//printf("Bar 3 Handoff State");
		lateralSolenoid1.Set(DoubleSolenoid::kForward);
		lateralSolenoid2.Set(DoubleSolenoid::kForward);
		stationaryClawSolenoid1.Set(DoubleSolenoid::kOff);
		stationaryClawSolenoid2.Set(DoubleSolenoid::kOff);
		armSolenoid1.Set(DoubleSolenoid::kOff);
		armSolenoid2.Set(DoubleSolenoid::kOff);
		if((BeamBreak1->GetVoltage() > beamBreakVoltageBand) )//&& (BeamBreak2->GetVoltage() > beamBreakVoltageBand))
		{
			stationaryClawSolenoid1.Set(DoubleSolenoid::kForward);
			stationaryClawSolenoid2.Set(DoubleSolenoid::kForward);
			armSolenoid1.Set(DoubleSolenoid::kForward);
		}
		if((Arm1ExtendedReedSwitch.Get() == 0) && (Arm2ExtendedReedSwitch.Get() == 0) && (StationaryArm1ExtendedSwitch.Get() == 0) && (StationaryArm2ExtendedSwitch.Get() == 0) && (LateralExtendedSwitch.Get() == 0))
		{
			DSIO.SetDigitalOutput(13, CustomDS::On);
		}
		break;
	case Done:
		//printf("Done State");
		lateralSolenoid1.Set(DoubleSolenoid::kOff);
		lateralSolenoid2.Set(DoubleSolenoid::kOff);
		stationaryClawSolenoid1.Set(DoubleSolenoid::kForward);
		stationaryClawSolenoid2.Set(DoubleSolenoid::kForward);
		armSolenoid1.Set(DoubleSolenoid::kReverse);
		armSolenoid2.Set(DoubleSolenoid::kReverse);
		if((Arm1RetractedReedSwitch->GetVoltage() < reedSensorVoltageBand) && (Arm2RetractedReedSwitch->GetVoltage() < reedSensorVoltageBand) && (StationaryArm1ExtendedSwitch.Get() == 0) && (StationaryArm2ExtendedSwitch.Get() == 0) && (LateralExtendedSwitch.Get() == 0))
		{
			DSIO.SetDigitalOutput(13, CustomDS::On);
		}
		break;
	case ManualOverride:
		//printf("Manual Override State");
		DSIO.SetDigitalOutput(16, CustomDS::On);
		break;
	default:
		DSIO.SetDigitalOutput(14, CustomDS::On);
		break;
	}
	//printf("State Machine %u\n", ClimbingStateMachine );
}

void CustomDS::HandleButtonBox()
{
	//This handles the LEDs on the Button box.
	if((ClimbingStateMachine == Floor) || (ClimbingStateMachine == ExtendToBar2) || (ClimbingStateMachine == ExtendToBar3))
	{
		DSIO.SetDigitalOutput(10,CustomDS::On);
	}
	else
	{
		DSIO.SetDigitalOutput(10,CustomDS::Off);
	}
	if((ClimbingStateMachine == HandoffBar1) || (ClimbingStateMachine == HandoffBar2) || (ClimbingStateMachine == HandoffBar3))
	{
		DSIO.SetDigitalOutput(11,CustomDS::On);
	}
	else
	{
		DSIO.SetDigitalOutput(11,CustomDS::Off);
	
	}
	if((ClimbingStateMachine == LiftToBar1) || (ClimbingStateMachine == LiftAndReleaseOffBar1) || (ClimbingStateMachine == LiftAndReleaseOffBar2))
	{
		DSIO.SetDigitalOutput(12,CustomDS::On);
	}
	else
	{
		DSIO.SetDigitalOutput(12, CustomDS::Off);
	}
	
	if(ClimbingStateMachine == ManualOverride)
	{
		if(digitals.Data.bits.solenoid1B == 1)
		{
			armSolenoid2.Set(DoubleSolenoid::kReverse);
		}
		else
		{
			armSolenoid2.Set(DoubleSolenoid::kForward);
		}
		if(digitals.Data.bits.solenoid1A == 1)
		{
			armSolenoid1.Set(DoubleSolenoid::kReverse);
		}
		else
		{
			armSolenoid1.Set(DoubleSolenoid::kForward);
		}
		if(digitals.Data.bits.solenoid2B == 1)
		{
			lateralSolenoid1.Set(DoubleSolenoid::kReverse);
			lateralSolenoid2.Set(DoubleSolenoid::kReverse);
		}
		else
		{
			lateralSolenoid1.Set(DoubleSolenoid::kForward);
			lateralSolenoid2.Set(DoubleSolenoid::kForward);
		}
		if(digitals.Data.bits.solenoid3B == 1)
		{
			stationaryClawSolenoid2.Set(DoubleSolenoid::kReverse);
		}
		else
		{
			stationaryClawSolenoid2.Set(DoubleSolenoid::kForward);
		}
		if(digitals.Data.bits.solenoid3A == 1)
		{
			stationaryClawSolenoid1.Set(DoubleSolenoid::kReverse);
		}
		else
		{
			stationaryClawSolenoid1.Set(DoubleSolenoid::kForward);
		}
	}
	if(digitals.Data.bits.solenoid2A == 1)
	{
		dumper->Set(0.0);
	}
	else
	{
		dumper->Set(0.25);
	}
	if(digitals.Data.bits.overrideToggle == 0)
	{
		ClimbingStateMachine = ManualOverride;
	}
	else
	{
		DSIO.SetDigitalOutput(16, CustomDS::Off);
	}
	if(this->GetPreviousButtonState())
	{
		ChangeState(false);
		if(digitals.Data.bits.prevState == 1)
		{
			prevlastState = true;
		}
		else
		{
			prevlastState = false;
		}
	}
	if(this->GetNextButtonState())
	{
		ChangeState(true);
		if(digitals.Data.bits.nextState == 1)
		{
			nextlastState = true;
		}
		else
		{
			nextlastState = false;
		}
	}
}
bool CustomDS::GetPreviousButtonState()
{
	
	if(digitals.Data.bits.prevState == 1)
	{
		prevones++;
	}
	else
	{
		prevzeros++;
	}
	if(prevones > 10 && prevzeros == 0)
	{
		prevones = 0;
		if(prevlastState == false)
		{
			return true;
		}
		else
		{
			return false;
		}
	}
	else if(prevones == 0 && prevzeros > 10)
	{
		prevzeros = 0;
		if(prevlastState == true)
		{
			return true;
		}
		else
		{
			return false;
		}
	}
	else if(prevones > 0 && prevzeros > 0)
	{
		return prevlastState;
	}
	else
	{
		return prevlastState;
	}
}
bool CustomDS::GetNextButtonState()
{
	
	if(digitals.Data.bits.nextState == 1)
	{
		nextones++;
	}
	else
	{
		nextzeros++;
	}
	if(nextones > 10 && nextzeros == 0)
	{
		nextones = 0;
		if(nextlastState == false)
		{
			return true;
		}
		else
		{
			return false;
		}
	}
	else if(nextones == 0 && nextzeros > 10)
	{
		nextzeros = 0;
		if(nextlastState == true)
		{
			return false;
		}
		else
		{
			return true;
		}
	}
	else if(nextones > 0 && nextzeros > 0)
	{
		return nextlastState;
	}
	else
	{
		return nextlastState;
	}
}
void CustomDS::UpdateDS()
{
	SmartDashboard::PutNumber("Left Motor", (int)(drive->GetLeftMotorSpeed() * 100));
	SmartDashboard::PutNumber("Right Motor", (int)(drive->GetRightMotorSpeed() * 100));
	SmartDashboard::PutString("State", GetStateToString());
	if((BeamBreak1->GetVoltage() > beamBreakVoltageBand) )//&& (BeamBreak2->GetVoltage() > beamBreakVoltageBand))
	{
		SmartDashboard::PutString("Beam Sensor", "Broken");
	}
	else
	{
		SmartDashboard::PutString("Beam Sensor", "Not Broken");
	}
	if(Arm1ExtendedReedSwitch.Get() == 0)
	{
		SmartDashboard::PutString("Arm Piston 1", "Extended");
	}
	else if(Arm1RetractedReedSwitch->GetVoltage() < reedSensorVoltageBand)
	{
		SmartDashboard::PutString("Arm Piston 1", "Retracted");
	}
	else
	{
		SmartDashboard::PutString("Arm Piston 1", "Unknown");
	}
	if(Arm2ExtendedReedSwitch.Get() == 0)
	{
		SmartDashboard::PutString("Arm Piston 2", "Extended");
	}
	else if(Arm2RetractedReedSwitch->GetVoltage() < reedSensorVoltageBand)
	{
		SmartDashboard::PutString("Arm Piston 2", "Retracted");
	}
	else
	{
		SmartDashboard::PutString("Arm Piston 2", "Unknown");
	}
	if(StationaryArm1ExtendedSwitch.Get() == 0)
	{
		SmartDashboard::PutString("Stationary Arm 1", "Extended");
	}
	else if(StationaryArm1RetractedSwitch.Get() == 0)
	{
		SmartDashboard::PutString("Stationary Arm 1", "Retracted");
	}
	else
	{
		SmartDashboard::PutString("Stationary Arm 1", "Unknown");
	}
	if(StationaryArm2ExtendedSwitch.Get() == 0)
	{
		SmartDashboard::PutString("Stationary Arm 2", "Extended");
	}
	else if(StationaryArm2RetractedSwitch.Get() == 0)
	{
		SmartDashboard::PutString("Stationary Arm 2", "Retracted");
	}
	else
	{
		SmartDashboard::PutString("Stationary Arm 2", "Unknown");
	}
	if(LateralExtendedSwitch.Get() == 0)
	{
		SmartDashboard::PutString("Lateral Pistons", "Extended");
	}
	else if(LateralRetractedSwitch.Get() == 0)
	{
		SmartDashboard::PutString("Lateral Pistons", "Retracted");
	}
	else
	{
		SmartDashboard::PutString("Lateral Pistons", "Unknown");
	}
}
	
void CustomDS::ChangeState(bool forward = false)
{
	DSIO.SetDigitalOutput(13, CustomDS::Off);
	switch(ClimbingStateMachine)
	{
	case Start:
		if(forward)
		{
			ClimbingStateMachine = Floor;
		}
		else
		{
			break;
		}
		break;
	case Floor:
		if(forward)
		{
			ClimbingStateMachine = LiftToBar1;
		}
		else
		{
			ClimbingStateMachine = Start;
		}
		break;
	case LiftToBar1:
		if(forward)
		{
			ClimbingStateMachine = HandoffBar1;
		}
		else
		{
			ClimbingStateMachine = Floor;
		}
		break;
	case HandoffBar1:
		if(forward)
		{
			ClimbingStateMachine = ExtendToBar2;
		}
		else
		{
			ClimbingStateMachine = LiftToBar1;
		}
		break;
	case ExtendToBar2:
		if(forward)
		{
			ClimbingStateMachine = LiftAndReleaseOffBar1;
		}
		else
		{
			ClimbingStateMachine = HandoffBar1;
		}
		break;
	case LiftAndReleaseOffBar1:
		if(forward)
		{
			ClimbingStateMachine = HandoffBar2;
		}
		else
		{
			ClimbingStateMachine = ExtendToBar2;
		}
		break;
	case HandoffBar2:
		if(forward)
		{
			ClimbingStateMachine = ExtendToBar3;
		}
		else
		{
			ClimbingStateMachine = LiftAndReleaseOffBar1;
		}
		break;
	case ExtendToBar3:
		if(forward)
		{
			ClimbingStateMachine = LiftAndReleaseOffBar2;
		}
		else
		{
			ClimbingStateMachine = HandoffBar2;
		}
		break;
	case LiftAndReleaseOffBar2:
		if(forward)
		{
			ClimbingStateMachine = HandoffBar3;
		}
		else
		{
			ClimbingStateMachine = ExtendToBar3;
		}
		break;
	case HandoffBar3:
		if(forward)
		{
			ClimbingStateMachine = Done;
		}
		else
		{
			ClimbingStateMachine = LiftAndReleaseOffBar2;
		}
		break;
	case Done:
		if(forward)
		{
			ClimbingStateMachine = Done;
		}
		else
		{
			ClimbingStateMachine = HandoffBar3;
		}
		break;
	case ManualOverride:
		ClimbingStateMachine = Start;
		break;
	default:
		// Still dont know
		break;
	}
}

string CustomDS::GetStateToString()
{
	switch(ClimbingStateMachine)
	{
	case Start:
		return "Start";
		break;
	case Floor:
		return "Floor";
		break;
	case LiftToBar1:
		return "Lifting to bar 1";
		break;
	case HandoffBar1:
		return "Hand off to bar 1";
		break;
	case ExtendToBar2:
		return "Extending to bar 2";
		break;
	case LiftAndReleaseOffBar1:
		return "Lifting to bar 2";
		break;
	case HandoffBar2:
		return "Hand off to bar 2";
		break;
	case ExtendToBar3:
		return "Extending to bar 3";
		break;
	case LiftAndReleaseOffBar2:
		return "Lifting to bar 3";
		break;
	case HandoffBar3:
		return "Hand off to bar 3";
		break;
	case Done:
		return "You're Done! =)";
		break;
	case ManualOverride:
		return "Manual Override";
		break;
	default:
		return "Invalid State!";
		break;
	}
}

CustomDS::~CustomDS()
{
	delete DS;
	delete BeamBreak1;
	delete BeamBreak2;
	delete Arm1RetractedReedSwitch;
	delete Arm2RetractedReedSwitch;
	delete dumper;
	delete drive;
}
